/*www.benjamin-dillenburger.com*/
package pGeom2D;

import java.util.List;
import java.util.Vector;

import processing.core.PVector;

public class PVecMath2D {
	public static int CWRIGHT = 1;
	public static int CCWLEFT = -1;
	public static int PARALLEL = 0;

	public static double dist(double x1, double y1, double x2, double y2) {
		return Math.sqrt(distSq(x1, y1, x2, y1));
	}

	public static double distSq(double x1, double y1, double x2, double y2) {
		double dX = x2 - x1;
		double dY = y2 - y1;
		return dX * dX + dY * dY;
	}

	public static PVector getCircleCenter(PVector pa, PVector pb, PVector pc) {
		// hier abfragen ob dreieck, wenn kein Dreieck dann Problem
		PVector vabO = rot90(PVector.sub(pa, pb));
		PVector vacO = rot90(PVector.sub(pa, pc));

		PVector pab = getCenter(pa, pb);
		PVector pac = getCenter(pa, pc);

		return lineLineIntersectionVec(pab, vabO, pac, vacO);
	}

	public static double[] getCircleCenter(double aX, double aY, double bX,
			double bY, double cX, double cY) {
		// hier abfragen ob dreieck, wenn kein Dreieck dann Problem
		double v1y = -aX + bX;
		double v1x = aY - bY;
		double v2y = -aX + cX;
		double v2x = aY - cY;

		double m1x = (aX + bX) * 0.5;
		double m1y = (aY + bY) * 0.5;
		double m2x = (aX + cX) * 0.5;
		double m2y = (aY + cY) * 0.5;

		return lineLineIntersectionVec(m1x, m1y, v1x, v1y, m2x, m2y, v2x, v2y);
	}

	public static PVector rot90(PVector a) {
		return new PVector(-a.y, a.x);
	}

	public static PVector rotM90(PVector a) {
		return new PVector(a.y, -a.x);
	}

	public static PVector getCenter(PVector pa, PVector pb) {
		return new PVector((pa.x + pb.x) / 2f, (pa.y + pb.y) / 2f);
	}

	public static PVector lineLineIntersection(PVector[] l1, PVector[] l2) {
		return lineLineIntersection(l1[0], l1[1], l2[0], l2[1]);

	}

	public static PVector lineLineIntersection(PVector pA, PVector pB,
			PVector pC, PVector pD) {
		PVector dir1 = PVector.sub(pB, pA);
		PVector dir2 = PVector.sub(pD, pC);
		return lineLineIntersectionVec(pA, dir1, pC, dir2);

	}

	public static double[] lineLineIntersectionVec(double aX, double aY,
			double v1x, double v1y, double bX, double bY, double v2x, double v2y) {
		double a1 = v1y;
		double b1 = -v1x;
		double c1 = a1 * aX + b1 * aY;
		// C = A*x1+B*y1
		double a2 = v2y;
		double b2 = -v2x;
		double c2 = a2 * bX + b2 * bY;

		double det = a1 * b2 - a2 * b1;
		System.out.println("parallel: " + Math.abs(det));
		if (Math.abs(det) < 0.00000001) {
			System.out.println("parallel");
			return null;
			// Lines are parallel
		} else {
			double x = (b2 * c1 - b1 * c2) / det;
			double y = (a1 * c2 - a2 * c1) / det;
			return new double[] { x, y };
		}

	}

	public static PVector lineLineIntersectionVec(PVector pA1, PVector dir1,
			PVector pB1, PVector dir2) {
		float a1 = dir1.y;
		float b1 = -dir1.x;
		float c1 = a1 * pA1.x + b1 * pA1.y;
		// C = A*x1+B*y1
		float a2 = dir2.y;
		float b2 = -dir2.x;
		float c2 = a2 * pB1.x + b2 * pB1.y;

		float det = a1 * b2 - a2 * b1;
		// System.out.println(": "+Math.abs(det));;
		if (Math.abs(det) < 0.01) {
			// System.out.println("parallel");
			return null;
			// Lines are parallel
		} else {
			float x = (b2 * c1 - b1 * c2) / det;
			float y = (a1 * c2 - a2 * c1) / det;
			return new PVector(x, y);
		}
	}

	static final float precision = 0.0001f;

	public static int relativeCCW(PVector p0, PVector p1, PVector p2) {
		double ar = parallelArea(p0, p1, p2);
		if (Math.abs(ar) < precision)
			return PARALLEL;
		if (ar > 0)
			return CCWLEFT;
		return CWRIGHT;
	}

	public static int relativeCCW(double x0, double y0, double x1, double y1,
			double x2, double y2) {
		double ar = parallelArea(x0, y0, x1, y1, x2, y2);
		if (Math.abs(ar) < precision)
			return PARALLEL;
		if (ar > 0)
			return CCWLEFT;
		return CWRIGHT;
	}

	public static double parallelArea(double x0, double y0, double x1,
			double y1, double x2, double y2) {
		double d1X = x0 - x1;
		double d1Y = y0 - y1;
		double d2X = x0 - x2;
		double d2Y = y0 - y2;
		return d1X * d2Y - d2X * d1Y;
	}

	public static double parallelArea(PVector p0, PVector p1, PVector p2) {
		PVector v1 = PVector.sub(p0, p1);
		PVector v2 = PVector.sub(p0, p2);
		return ((v1.x) * (v2.y) - (v2.x) * (v1.y));
	}

	static public boolean contains(PVector p1, PVector p2, PVector p3, PVector p) {
		if (relativeCCW(p1, p2, p) == CWRIGHT)
			return false;
		if (relativeCCW(p2, p3, p) == CWRIGHT)
			return false;
		if (relativeCCW(p3, p1, p) == CWRIGHT)
			return false;
		return true;
	}

	static public PVector getOffset(PVector p1, PVector p2, PVector p3,
			float offset1, float offset2) {
		PVector[] line1 = getParallelLine(p1, p2, offset1);
		PVector[] line2 = getParallelLine(p2, p3, offset2);
		PVector oP = lineLineIntersection(line1[0], line1[1], line2[0],
				line2[1]);
		if (oP == null)
			return line1[1];
		else
			return oP;
	}

	static public PVector getOffset(PVector p1, PVector p2, PVector p3,
			float offset) {

		PVector[] line1 = getParallelLine(p1, p2, offset);
		PVector[] line2 = getParallelLine(p2, p3, offset);
		PVector oP = lineLineIntersection(line1[0], line1[1], line2[0],
				line2[1]);
		if (oP == null)
			return line1[1];
		else
			return oP;
		/*
		 * PVector v1 = PVector.sub(p1, p2); v1.normalize(); v1.mult(offset);
		 * PVector v2 = PVector.sub(p3, p2); v2.normalize(); v2.mult(offset);
		 * PVector mV = PVector.mult(PVector.add(v1, v2),0.5f); PVector
		 * t=PVector.mult(PVector.sub(v2, v1),0.5f); float l = (offset * offset)
		 * / t.mag(); mV.normalize(); mV.mult(l); return PVector.add(mV, p2);
		 */
	}

	public static PVector rotateZ0(PVector p, float angle) {
		float rotX = (float) (p.x * Math.cos(angle) - p.y * Math.sin(angle));
		float rotY = (float) (p.x * Math.sin(angle) + p.y * Math.cos(angle));
		return new PVector(rotX, rotY, p.z);
	}

	public static Vector<PVector> translate(List<PVector> points, float x,
			float y, float z) {
		Vector<PVector> tPoints = new Vector<PVector>();
		for (int i = 0; i < points.size(); i++) {
			PVector p = points.get(i);
			tPoints.add(new PVector(p.x + x, p.y + y, p.z + z));
		}
		return tPoints;
	}

	public static Vector<PVector> rotateZ0(List<PVector> points, float angle) {
		Vector<PVector> rotPoints = new Vector<PVector>();
		for (int i = 0; i < points.size(); i++) {
			PVector p = points.get(i);
			rotPoints.add(rotateZ0(p, angle));
		}
		return rotPoints;
	}

	static public Vector<PVector> getOffsetPolyLine(List<PVector> points,
			float offset) {
		Vector<PVector> offsetPoints = new Vector<PVector>();

		for (int i = 1; i < points.size() - 1; i++) {
			PVector pt0 = points.get(i - 1);
			PVector pt1 = points.get(i);
			PVector pt2 = points.get(i + 1);
			if (i == 1) {
				PVector[] line = PVecMath2D.getParallelLine(pt0, pt1, offset);
				offsetPoints.add(line[0]);
			}
			PVector oP = PVecMath2D.getOffset(pt0, pt1, pt2, offset);
			offsetPoints.add(oP);
			if (i == points.size() - 2) {
				PVector[] line = PVecMath2D.getParallelLine(pt1, pt2, offset);
				offsetPoints.add(line[1]);
			}
		}
		return offsetPoints;
	}

	static public Vector<PVector> getOffset(List<PVector> pts,
			List<Float> offsets) {
		Vector<PVector> offsetPts = new Vector<PVector>();
		int prev, next;
		for (int j = 0; j < pts.size(); j++) {
			prev = j - 1;
			if (prev < 0)
				prev = pts.size() - 1;
			next = j + 1;
			if (next >= pts.size())
				next = 0;
			PVector p1 = (PVector) pts.get(prev);
			PVector p2 = (PVector) pts.get(j);
			PVector p3 = (PVector) pts.get(next);
			float o1 = offsets.get(prev);
			float o2 = offsets.get(j);
			offsetPts.add(getOffset(p1, p2, p3, o1, o2));
		}
		return offsetPts;
	}

	static public Vector<PVector> getOffset(Vector<PVector> pts, float[] offset) {
		Vector<PVector> offsetPts = new Vector<PVector>();
		int prev, next;
		for (int j = 0; j < pts.size(); j++) {
			prev = j - 1;
			if (prev < 0)
				prev = pts.size() - 1;
			next = j + 1;
			if (next >= pts.size())
				next = 0;
			float o1 = offset[prev];
			float o2 = offset[j];
			PVector p1 = (PVector) pts.get(prev);
			PVector p2 = (PVector) pts.get(j);
			PVector p3 = (PVector) pts.get(next);
			offsetPts.add(getOffset(p1, p2, p3, o1, o2));
		}
		return offsetPts;
	}

	static public Vector<PVector> getOffset(List<PVector> pts, float offset) {
		Vector<PVector> offsetPts = new Vector<PVector>();
		int prev, next;
		for (int j = 0; j < pts.size(); j++) {
			prev = j - 1;
			if (prev < 0)
				prev = pts.size() - 1;
			next = j + 1;
			if (next >= pts.size())
				next = 0;
			PVector p1 = (PVector) pts.get(prev);
			PVector p2 = (PVector) pts.get(j);
			PVector p3 = (PVector) pts.get(next);
			offsetPts.add(getOffset(p1, p2, p3, offset));
		}
		return offsetPts;
	}

	public static PVector[] getParallelLine(PVector p1, PVector p2, float offset) {
		PVector[] line = new PVector[2];
		PVector v = PVector.sub(p2, p1);
		v = rotM90(v);
		v.normalize();
		v.mult(offset);
		line[0] = PVector.add(p1, v);
		line[1] = PVector.add(p2, v);
		return line;
	}

	public static PVector getPolar(PVector p, double theta, double length) {
		double x = length * Math.cos(theta) + p.x;
		double y = length * Math.sin(theta) + p.y;

		return new PVector((float) x, (float) y, 0);
	}

	public static PVector getPolar(float theta, float length) {
		double x = length * Math.cos(theta);
		double y = length * Math.sin(theta);

		return new PVector((float) x, (float) y, 0);
	}

	public static float getAngle(float x, float y) {
		return (float) Math.atan2(y, x);
	}

	public static float getAngle(PVector v) {
		return getAngle(v.x, v.y);
	}

	public static double getAngleBetweenCCW(PVector p, PVector a, PVector b) {
		PVector va = PVector.sub(p, a);
		PVector vb = PVector.sub(p, b);
		return getAngleBetweenCCW(Math.atan2(vb.y, vb.x),
				Math.atan2(va.y, va.x));

	}

	public static double getAngleBetween(double theta1, double theta2) {
		double delta = theta2 - theta1;
		if (delta > Math.PI) {
			return delta - (2.0 * Math.PI);
		}
		if (delta < -Math.PI) {
			return (2.0 * Math.PI) + delta;
		}
		return delta;
	}

	public static double getAngleBetweenCCW(double theta1, double theta2) {
		double cAngle = theta1 - theta2;
		if (cAngle < 0) {
			return cAngle + 2.0 * Math.PI;
		}
		return cAngle;
	}

	public static Vector<PVector> getRectAsPoly(float x, float y, float w,
			float h) {
		Vector<PVector> points = new Vector<PVector>();
		points.add(new PVector(x, y, 0));
		points.add(new PVector(x + w, y, 0));
		points.add(new PVector(x + w, y + h, 0));
		points.add(new PVector(x, y + h, 0));
		return points;
	}

	public static float getArea(List<? extends PVector> points) {

		float area = 0;
		int j;
		for (int i = 0; i < points.size(); i++) {
			j = i + 1;
			if (j == points.size())
				j = 0;
			PVector p1 = points.get(i);
			PVector p2 = points.get(j);
			area += (p1.x + p2.x) * (p2.y - p1.y);
		}
		return area / 2f;
	}

}
